package principal;

import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.ColorSensor.Color;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.SensorPort;
import lejos.util.Delay;

/**
 * Subsistema de lançamento das bolas.
 * 
 * @author SEK
 * 
 */
public class NXTLancamento implements Runnable {

	// Constantes
	private static final int REPOUSO = 0;
	private static final int ATUALIZANDO = 1;
	private static final int LANCANDO = 2;
	private static final int AJUSTANDO = 3;

	public static final boolean DIREITA = false;
	public static final boolean ESQUERDA = true;

	private static final int angBatida = 47+44; //47
	private static final int angRetorno = 73-44; //73

	private static final int velBatida = 950;
	private static final int velRetorno = 80;
	
	public static boolean fim = false;

	//Componentes do robô
	private ColorSensor sensorCor;
	private NXTRegulatedMotor motorTaco;

	//Variáveis de estado
	private boolean temBolinha;
	private boolean bolaAzul;
	private boolean bolaLancada;
	private int SubEstado;
	private boolean posicaoTaco;

	/**
	 * Método get para a variável temBolinha
	 * 
	 * @param null
	 * @return variável temBolinha
	 */
	public boolean get_temBolinha() {
		return this.temBolinha;
	}

	/**
	 * Método get para a variável bolaAzul
	 * 
	 * @param null
	 * @return variável bolaAzul
	 */
	public boolean get_bolaAzul() {
		return this.bolaAzul;
	}

	/**
	 * Método get para a variável bolaLançada
	 * 
	 * @param null
	 * @return variável bolaLancada
	 */
	public boolean get_bolaLancada() {
		return this.bolaLancada;
	}

	/**
	 * Constroi um subsistema de lançamento do robô
	 * 
	 * @param s1
	 *            Porta de ligação do sensor de cor
	 * @param m1
	 *            Porta de ligação do motor do taco
	 * @return null
	 */
	public NXTLancamento(SensorPort s1, NXTRegulatedMotor m1) {
		this.sensorCor = new ColorSensor(s1);
		this.motorTaco = m1;
		
		this.bolaAzul = false;
		this.temBolinha = false;
		this.bolaLancada = true;
		this.posicaoTaco = DIREITA;
		this.SubEstado = REPOUSO;

		motorTaco.suspendRegulation();
		LCD.drawString("Ajuste do taco:", 1, 1);
		while (!Button.ENTER.isDown()) {
			Delay.msDelay(100);
			LCD.clear(3);
			if (motorTaco.getTachoCount() >= 0) {
				posicaoTaco = ESQUERDA;
				LCD.drawString("ESQUERDA", 1, 3);
			} else {
				posicaoTaco = DIREITA;
				LCD.drawString("DIREITA", 1, 3);
			}
		}
		motorTaco.resetTachoCount();
		LCD.clear();
	}

	/**
	 * Método no qual o subsistema de lançamento executará suas rotinas de
	 * amostragem do sensor de cor e atualização das variáveis correspondentes
	 */
	@Override
	public void run() {
		// TODO Auto-generated method stub
		while (!fim) {
			amostrarSensorCor();
			Thread.yield();
			Delay.msDelay(1000);
		}
	}

	/**
	 * Método de lançamento de bolas laranjas
	 *
	 * @param null
	 * @return Sucesso ou não do lançamento
	 */
	public boolean lancarBolaLaranja() {
		while (SubEstado != REPOUSO)
			;
		SubEstado = LANCANDO;
		motorTaco.setSpeed(velBatida);
		if (posicaoTaco == DIREITA) {
			motorTaco.rotate(angBatida);
			Delay.msDelay(500);
			motorTaco.setSpeed(velRetorno);
			motorTaco.rotate(angRetorno);
		} else { // if(posicaoTaco == ESQUERDA)
			motorTaco.rotate(-angBatida);
			Delay.msDelay(500);
			motorTaco.setSpeed(velRetorno);
			motorTaco.rotate(-angRetorno);
		}
		bolaLancada = true;
		SubEstado = REPOUSO;
		return true;
	}

	/**
	 * Método de lançamento de bolas azuis
	 *
	 * @param null
	 * @return Sucesso ou não do lançamento
	 */
	public boolean lancarBolaAzul() {
		while (SubEstado != REPOUSO)
			;
		SubEstado = LANCANDO;
		motorTaco.setSpeed(80);
		if (posicaoTaco == DIREITA) {
			motorTaco.rotate(-angBatida - angRetorno);
		} else { // if(posicaoTaco == ESQUERDA)
			motorTaco.rotate(angBatida + angRetorno);
		}
		bolaLancada = true;
		SubEstado = REPOUSO;
		return true;
	}

	/**
	 * Método que efetua a leitura e tratamento do sensor de cor. Ocorre também
	 * a atualização das variáveis correspondentes.
	 * 
	 * @param null
	 * @return Sucesso ou não da leitura do sensor
	 */
	private boolean amostrarSensorCor() {
		while (SubEstado != REPOUSO)
			;
		SubEstado = ATUALIZANDO;
		Color c = sensorCor.getColor();
		
		System.out.println(c.getRed());

		if (c.getRed() < 20) {
			temBolinha = false;
		} else if (c.getRed() >= 35) {
			bolaAzul = false;
			temBolinha = true;
			bolaLancada = false;
		}
		SubEstado = REPOUSO;
		return true;
	}

	/**
	 * Método que ajusta a posição do taco de acordo com a orientação do robô
	 * 
	 * @param pos
	 *            posição na qual o taco deve ser posicionado
	 * @return Sucesso ou não do reposicionamento
	 */
	public void ajustarTaco(boolean pos) {
		while (SubEstado != REPOUSO)
			;
		SubEstado = AJUSTANDO;
		motorTaco.setSpeed(velRetorno);
		if (pos == DIREITA) {
			motorTaco.rotate(angRetorno);
		} else { // if(pos == ESQUERDA)
			motorTaco.rotate(-angRetorno);
		}
		posicaoTaco = pos;
		SubEstado = REPOUSO;
	}
}